#!/usr/bin/env python3

import sys
import math
import time
import threading

import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64MultiArray, Float64
from sensor_msgs.msg import JointState

from PySide6.QtCore import Qt, QTimer
from PySide6.QtGui import QFont, QShortcut, QKeySequence, QMouseEvent
from PySide6.QtWidgets import (
    QApplication, QMainWindow, QWidget, QVBoxLayout, QHBoxLayout, QLabel,
    QPushButton, QSlider, QDoubleSpinBox, QScrollArea, QGroupBox, QCheckBox,
    QStatusBar, QSizePolicy, QMenuBar
)
import qtawesome as qta


class ClickableSlider(QSlider):
    """Clickable slider that jumps to the clicked position"""
    def __init__(self, orientation):
        super().__init__(orientation)
        
    def mousePressEvent(self, event: QMouseEvent):
        if event.button() == Qt.LeftButton:
            if self.orientation() == Qt.Horizontal:
                slider_min = self.minimum()
                slider_max = self.maximum()
                widget_width = self.width()
                handle_width = 20
                usable_width = widget_width - handle_width
                
                if usable_width > 0:
                    relative_pos = (event.x() - handle_width // 2) / usable_width
                    relative_pos = max(0.0, min(1.0, relative_pos))
                    new_value = slider_min + relative_pos * (slider_max - slider_min)
                    self.setValue(int(new_value))
            else:
                slider_min = self.minimum()
                slider_max = self.maximum()
                widget_height = self.height()
                handle_height = 20
                usable_height = widget_height - handle_height
                
                if usable_height > 0:
                    relative_pos = 1.0 - (event.y() - handle_height // 2) / usable_height
                    relative_pos = max(0.0, min(1.0, relative_pos))
                    new_value = slider_min + relative_pos * (slider_max - slider_min)
                    self.setValue(int(new_value))
        
        super().mousePressEvent(event)


class ThemeManager:
    @staticmethod
    def apply_light_theme(app):
        app.setFont(QFont("Inter", 14))  # Increased font size
        app.setStyleSheet("""
            QMainWindow {
                background-color: #F7F7F7;
                color: #333333;
                font-size: 15px;
            }
            QWidget {
                background-color: #F7F7F7;
                color: #333333;
                font-size: 15px;
            }
            QGroupBox {
                background-color: rgba(255,255,255,0.9);
                border: 1px solid #E0E0E0;
                border-radius: 8px;
                margin-top: 25px;
                padding: 20px 16px 16px 16px;
                font-weight: 600;
                color: #333333;
            }
            QGroupBox::title {
                subcontrol-origin: border;
                subcontrol-position: top left;
                left: 20px;
                top: 10px;
                padding: 2px 8px;
                background-color: #F7F7F7;
                color: #333333;
                font-weight: 600;
                font-size: 15px;
                border-radius: 4px;
            }
            QPushButton {
                background-color: #4A90E2;
                color: #FFFFFF;
                border: none;
                border-radius: 8px;
                padding: 10px 20px;
                font-size: 15px;
                font-weight: 500;
                min-height: 32px;
            }
            QPushButton:hover {
                background-color: #357ABD;
            }
            QPushButton:pressed {
                background-color: #2A6099;
            }
            QPushButton[secondary="true"] {
                background-color: #E0E0E0;
                color: #333333;
            }
            QPushButton[secondary="true"]:hover {
                background-color: #D0D0D0;
            }
            QPushButton[secondary="true"]:pressed {
                background-color: #C0C0C0;
            }
            QLabel {
                color: #333333;
                background-color: transparent;
                font-size: 15px;
            }
            QSlider {
                background-color: transparent;
                min-height: 32px;
            }
            QSlider::groove:horizontal {
                border: none;
                height: 6px;
                background: #E0E0E0;
                border-radius: 3px;
                margin: 12px 0;
            }
            QSlider::handle:horizontal {
                background: #4A90E2;
                border: none;
                width: 20px;
                height: 20px;
                margin: -7px 0;
                border-radius: 10px;
            }
            QSlider::handle:horizontal:hover {
            }
            QSlider::sub-page:horizontal {
                background: #4A90E2;
                border-radius: 3px;
                margin: 12px 0;
            }
            QDoubleSpinBox {
                background-color: #FFFFFF;
                color: #333333;
                border: 1px solid #E0E0E0;
                border-radius: 8px;
                padding: 6px;
                font-size: 15px;
            }
            QDoubleSpinBox::up-button, QDoubleSpinBox::down-button {
                background-color: #F0F0F0;
                border: none;
                border-left: 1px solid #E0E0E0;
                width: 24px;
            }
            QDoubleSpinBox::up-button {
                border-top-right-radius: 8px;
            }
            QDoubleSpinBox::down-button {
                border-bottom-right-radius: 8px;
            }
            QDoubleSpinBox::up-button:hover, QDoubleSpinBox::down-button:hover {
                background-color: #E0E0E0;
            }
            QDoubleSpinBox::up-arrow, QDoubleSpinBox::down-arrow {
                width: 10px;
                height: 10px;
            }
            QCheckBox {
                color: #333333;
                background-color: transparent;
                font-size: 15px;
            }
            QStatusBar {
                background-color: #F0F0F0;
                color: #333333;
                border-top: 1px solid #E0E0E0;
                font-size: 15px;
            }
            QMenuBar {
                background-color: #F0F0F0;
                color: #333333;
                border-bottom: 1px solid #E0E0E0;
                font-size: 15px;
            }
            QMenuBar::item {
                background-color: transparent;
                color: #333333;
                padding: 6px 12px;
            }
            QMenuBar::item:selected {
                background-color: #E0E0E0;
            }
            QMenu {
                background-color: #FFFFFF;
                color: #333333;
                border: 1px solid #E0E0E0;
                font-size: 15px;
            }
            QMenu::item {
                background-color: transparent;
                color: #333333;
                padding: 6px 24px;
            }
            QMenu::item:selected {
                background-color: #E0E0E0;
            }
            QScrollArea {
                background-color: #F7F7F7;
                border: none;
            }
            QScrollArea > QWidget > QWidget {
                background-color: #F7F7F7;
            }
            QScrollBar:vertical {
                background-color: #F0F0F0;
                width: 12px;
                border-radius: 6px;
                margin: 0;
                border: none;
            }
            QScrollBar::handle:vertical {
                background-color: #C0C0C0;
                border-radius: 6px;
                min-height: 20px;
                margin: 2px;
                border: none;
            }
            QScrollBar::handle:vertical:hover {
                background-color: #A0A0A0;
            }
            QScrollBar::handle:vertical:pressed {
                background-color: #808080;
            }
            QScrollBar::add-line:vertical, QScrollBar::sub-line:vertical {
                border: none;
                background: none;
                height: 0px;
                border-radius: 0px;
            }
            QScrollBar::add-page:vertical, QScrollBar::sub-page:vertical {
                background: none;
                border: none;
                border-radius: 6px;
            }
            QScrollBar:horizontal {
                background-color: #F0F0F0;
                height: 12px;
                border-radius: 6px;
                margin: 0;
                border: none;
            }
            QScrollBar::handle:horizontal {
                background-color: #C0C0C0;
                border-radius: 6px;
                min-width: 20px;
                margin: 2px;
                border: none;
            }
            QScrollBar::handle:horizontal:hover {
                background-color: #A0A0A0;
            }
            QScrollBar::handle:horizontal:pressed {
                background-color: #808080;
            }
            QScrollBar::add-line:horizontal, QScrollBar::sub-line:horizontal {
                border: none;
                background: none;
                width: 0px;
                border-radius: 0px;
            }
            QScrollBar::add-page:horizontal, QScrollBar::sub-page:horizontal {
                background: none;
                border: none;
                border-radius: 6px;
            }
        """)
    
    @staticmethod
    def apply_dark_theme(app):
        app.setFont(QFont("Inter", 14))
        app.setStyleSheet("""
            QMainWindow {
                background-color: #1E1E1E;
                color: #E0E0E0;
                font-size: 15px;
            }
            QWidget {
                background-color: #1E1E1E;
                color: #E0E0E0;
                font-size: 15px;
            }
            QGroupBox {
                background-color: rgba(40,40,40,0.9);
                border: 1px solid #555555;
                border-radius: 8px;
                margin-top: 25px;
                padding: 20px 16px 16px 16px;
                font-weight: 600;
                color: #E0E0E0;
            }
            QGroupBox::title {
                subcontrol-origin: border;
                subcontrol-position: top left;
                left: 20px;
                top: 10px;
                padding: 2px 8px;
                background-color: #1E1E1E;
                color: #E0E0E0;
                font-weight: 600;
                font-size: 15px;
                border-radius: 4px;
            }
            QPushButton {
                background-color: #4A90E2;
                color: #FFFFFF;
                border: none;
                border-radius: 8px;
                padding: 10px 20px;
                font-size: 15px;
                font-weight: 500;
                min-height: 32px;
            }
            QPushButton:hover {
                background-color: #357ABD;
            }
            QPushButton:pressed {
                background-color: #2A6099;
            }
            QPushButton[secondary="true"] {
                background-color: #555555;
                color: #E0E0E0;
            }
            QPushButton[secondary="true"]:hover {
                background-color: #666666;
            }
            QPushButton[secondary="true"]:pressed {
                background-color: #444444;
            }
            QLabel {
                color: #E0E0E0;
                background-color: transparent;
                font-size: 15px;
            }
            QSlider {
                background-color: transparent;
                min-height: 32px;
            }
            QSlider::groove:horizontal {
                border: none;
                height: 6px;
                background: #555555;
                border-radius: 3px;
                margin: 12px 0;
            }
            QSlider::handle:horizontal {
                background: #4A90E2;
                border: none;
                width: 20px;
                height: 20px;
                margin: -7px 0;
                border-radius: 10px;
            }
            QSlider::handle:horizontal:hover {
            }
            QSlider::sub-page:horizontal {
                background: #4A90E2;
                border-radius: 3px;
                margin: 12px 0;
            }
            QDoubleSpinBox {
                background-color: #2B2B2B;
                color: #E0E0E0;
                border: 1px solid #555555;
                border-radius: 8px;
                padding: 6px;
                font-size: 15px;
            }
            QDoubleSpinBox::up-button, QDoubleSpinBox::down-button {
                background-color: #404040;
                border: none;
                border-left: 1px solid #555555;
                width: 24px;
            }
            QDoubleSpinBox::up-button {
                border-top-right-radius: 8px;
            }
            QDoubleSpinBox::down-button {
                border-bottom-right-radius: 8px;
            }
            QDoubleSpinBox::up-button:hover, QDoubleSpinBox::down-button:hover {
                background-color: #505050;
            }
            QDoubleSpinBox::up-arrow, QDoubleSpinBox::down-arrow {
                width: 10px;
                height: 10px;
            }
            QCheckBox {
                color: #E0E0E0;
                background-color: transparent;
                font-size: 15px;
            }
            QStatusBar {
                background-color: #2B2B2B;
                color: #E0E0E0;
                border-top: 1px solid #555555;
                font-size: 15px;
            }
            QMenuBar {
                background-color: #2B2B2B;
                color: #E0E0E0;
                border-bottom: 1px solid #555555;
                font-size: 15px;
            }
            QMenuBar::item {
                background-color: transparent;
                color: #E0E0E0;
                padding: 6px 12px;
            }
            QMenuBar::item:selected {
                background-color: #555555;
            }
            QMenu {
                background-color: #2B2B2B;
                color: #E0E0E0;
                border: 1px solid #555555;
                font-size: 15px;
            }
            QMenu::item {
                background-color: transparent;
                color: #E0E0E0;
                padding: 6px 24px;
            }
            QMenu::item:selected {
                background-color: #555555;
            }
            QScrollArea {
                background-color: #1E1E1E;
                border: none;
            }
            QScrollArea > QWidget > QWidget {
                background-color: #1E1E1E;
            }
            QScrollBar:vertical {
                background-color: #2B2B2B;
                width: 12px;
                border-radius: 6px;
                margin: 0;
                border: none;
            }
            QScrollBar::handle:vertical {
                background-color: #555555;
                border-radius: 6px;
                min-height: 20px;
                margin: 2px;
                border: none;
            }
            QScrollBar::handle:vertical:hover {
                background-color: #666666;
            }
            QScrollBar::handle:vertical:pressed {
                background-color: #777777;
            }
            QScrollBar::add-line:vertical, QScrollBar::sub-line:vertical {
                border: none;
                background: none;
                height: 0px;
                border-radius: 0px;
            }
            QScrollBar::add-page:vertical, QScrollBar::sub-page:vertical {
                background: none;
                border: none;
                border-radius: 6px;
            }
            QScrollBar:horizontal {
                background-color: #2B2B2B;
                height: 12px;
                border-radius: 6px;
                margin: 0;
                border: none;
            }
            QScrollBar::handle:horizontal {
                background-color: #555555;
                border-radius: 6px;
                min-width: 20px;
                margin: 2px;
                border: none;
            }
            QScrollBar::handle:horizontal:hover {
                background-color: #666666;
            }
            QScrollBar::handle:horizontal:pressed {
                background-color: #777777;
            }
            QScrollBar::add-line:horizontal, QScrollBar::sub-line:horizontal {
                border: none;
                background: none;
                width: 0px;
                border-radius: 0px;
            }
            QScrollBar::add-page:horizontal, QScrollBar::sub-page:horizontal {
                background: none;
                border: none;
                border-radius: 6px;
            }
        """)


class JointControlNode(Node):
    def __init__(self):
        super().__init__('joint_control_node')
        self.joint_command_publisher = self.create_publisher(Float64MultiArray, '/command_move_joint', 10)
        self.gripper_position_publisher = self.create_publisher(Float64, '/command_gripper_position', 10)
        self.gripper_ff_publisher = self.create_publisher(Float64, '/command_gripper_ff_torque', 10)
        self.joint_state_sub = self.create_subscription(JointState, '/joint_states', self._joint_state_cb, 10)
        self.preview_pub = self.create_publisher(JointState, '/preview_joint_states', 10)
        self._preview_rate_hz = 30.0
        self._last_preview_pub_time = 0.0
        self.joint_names = ['joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7']
        self.joint_limits = [
            (-2.967, 2.967),
            (-1.9199, 1.9199),
            (-2.967, 2.967),
            (-2.094, 2.094),
            (-2.967, 2.967),
            (-2.904, 2.904),
            (-2.967, 2.967),
        ]
        self.gripper_position_range = (0.0, 92.0)
        self.current_positions = [0.0] * 9
        self.target_positions = [0.0] * 7
        self.target_gripper_position = 0.0
        self.target_gripper_ff_torque = 0.0

    def _joint_state_cb(self, msg: JointState):
        try:
            if len(msg.position) >= 9:
                self.current_positions = list(msg.position[:9])
        except Exception as e:
            self.get_logger().warning(f"Joint state callback error: {e}")

    def _build_preview_joint_state(self):
        names = [f'ghost_joint{i}' for i in range(1, 10)]
        pos = list(self.target_positions[:7])
        mm_total = max(0.0, min(92.0, float(self.target_gripper_position)))
        meters = max(0.0, min(0.046, (mm_total * 0.001) / 2.0))
        pos.append(meters)
        pos.append(-meters)
        msg = JointState()
        msg.name = names
        msg.position = pos
        msg.header.stamp = self.get_clock().now().to_msg()
        return msg

    def publish_preview_state(self, force: bool = False):
        now = time.monotonic()
        if force or (now - self._last_preview_pub_time) >= (1.0 / self._preview_rate_hz):
            try:
                if rclpy.ok():
                    m = self._build_preview_joint_state()
                    self.preview_pub.publish(m)
                    self._last_preview_pub_time = now
            except Exception as e:
                self.get_logger().warning(f"Failed to publish preview: {e}")

    def send_joint_command(self, positions):
        try:
            msg = Float64MultiArray()
            msg.data = list(positions[:7])
            self.joint_command_publisher.publish(msg)
            self.get_logger().info(f"Published joint command: {msg.data}")
        except Exception as e:
            self.get_logger().error(f"Failed send_joint_command: {e}")

    def send_gripper_command(self, mm, ff_nm=0.0):
        try:
            ff_msg = Float64()
            ff_msg.data = max(-3.0, min(3.0, float(ff_nm)))
            pos_msg = Float64()
            pos_msg.data = max(0.0, min(92.0, float(mm)))
            self.gripper_ff_publisher.publish(ff_msg)
            self.gripper_position_publisher.publish(pos_msg)
            self.get_logger().info(f"Sent gripper pos {pos_msg.data}mm, ff {ff_msg.data}Nm")
        except Exception as e:
            self.get_logger().error(f"Failed send_gripper_command: {e}")


class JointControlWindow(QMainWindow):
    def __init__(self, node: JointControlNode, shutdown_event: threading.Event):
        super().__init__()
        self.node = node
        self.shutdown_event = shutdown_event
        self.setWindowTitle("Zoros Robot Joint Control")
        self.setMinimumSize(900, 1400)

        self.current_theme = "light"
        self.live_gripper_enabled = False
        self._last_gripper_pub_time = 0.0
        self._live_gripper_rate_hz = 20.0

        self._setup_menu_bar()
        self._setup_shortcuts()

        central = QWidget()
        self.setCentralWidget(central)
        main_layout = QVBoxLayout(central)
        main_layout.setContentsMargins(16, 16, 16, 16)
        main_layout.setSpacing(20)

        header_layout = QHBoxLayout()
        header = QLabel("Zoros Robot Joint Control")
        header.setStyleSheet("font-weight: bold; font-size: 22px; color: #333333;")
        header_layout.addWidget(header)
        self.connection_status = QLabel("")
        self.connection_status.setPixmap(qta.icon('mdi.circle', color='red').pixmap(16, 16))
        self.connection_status.setStyleSheet("padding: 8px;")
        header_layout.addStretch()
        header_layout.addWidget(self.connection_status)
        main_layout.addLayout(header_layout)

        scroller = QScrollArea()
        scroller.setWidgetResizable(True)
        main_layout.addWidget(scroller, 1)

        content = QWidget()
        scroller.setWidget(content)
        content_layout = QVBoxLayout(content)
        content_layout.setSpacing(8)
        content_layout.setContentsMargins(8, 8, 8, 8)

        self.joint_spinboxes = []
        self.joint_sliders = []
        self.current_labels = []
        self.target_labels = []

        for i, name in enumerate(self.node.joint_names):
            box = QGroupBox(f"{name} (rad)")
            box_layout = QVBoxLayout(box)
            box_layout.setSpacing(12)
            box_layout.setContentsMargins(12, 25, 12, 12)
            row_top = QHBoxLayout()
            cur_label = QLabel("Current: 0.000")
            cur_label.setFixedWidth(140)
            cur_label.setStyleSheet("color: #666666;")
            row_top.addWidget(cur_label)
            self.current_labels.append(cur_label)

            tgt_label = QLabel("Target: 0.000")
            tgt_label.setFixedWidth(140)
            tgt_label.setStyleSheet("color: #666666;")
            row_top.addWidget(tgt_label)
            self.target_labels.append(tgt_label)

            deg_btn = QPushButton("Reset")
            deg_btn.setToolTip("Reset this joint to zero")
            deg_btn.setFixedWidth(100)
            deg_btn.setProperty("secondary", True)
            row_top.addWidget(deg_btn)
            row_top.addStretch()
            box_layout.addLayout(row_top)

            row = QHBoxLayout()
            min_val, max_val = self.node.joint_limits[i]
            slider = ClickableSlider(Qt.Horizontal)
            slider.setRange(0, 10000)
            slider.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Fixed)
            spin = QDoubleSpinBox()
            spin.setDecimals(4)
            spin.setRange(min_val, max_val)
            spin.setSingleStep(0.001)
            spin.setValue(0.0)
            spin.setFixedWidth(140)

            def slider_to_val(sl):
                return min_val + (max_val - min_val) * (sl / 10000.0)

            def val_to_slider(v):
                return int(round((v - min_val) / (max_val - min_val) * 10000.0))

            slider.setValue(val_to_slider(0.0))

            slider.valueChanged.connect(lambda v, idx=i, s2v=slider_to_val, sp=spin: self._on_slider_changed(idx, v, s2v, sp))
            spin.valueChanged.connect(lambda v, idx=i, v2s=val_to_slider, sl=slider: self._on_spin_changed(idx, v, v2s, sl))
            deg_btn.clicked.connect(lambda checked, idx=i, sp=spin, sl=slider: self._on_reset_joint(idx, sp, sl))

            row.addWidget(slider)
            row.addWidget(spin)
            box_layout.addLayout(row)
            content_layout.addWidget(box)
            self.joint_sliders.append(slider)
            self.joint_spinboxes.append(spin)

        grip_box = QGroupBox("Gripper Control (mm)")
        grip_layout = QVBoxLayout(grip_box)
        grip_layout.setSpacing(12)
        grip_layout.setContentsMargins(12, 25, 12, 12)

        grip_top = QHBoxLayout()
        self.gripper_current_label = QLabel("Current: 0.0")
        self.gripper_current_label.setFixedWidth(140)
        self.gripper_current_label.setStyleSheet("color: #666666;")
        self.gripper_target_label = QLabel("Target: 0.0")
        self.gripper_target_label.setFixedWidth(140)
        self.gripper_target_label.setStyleSheet("color: #666666;")
        grip_top.addWidget(self.gripper_current_label)
        grip_top.addWidget(self.gripper_target_label)

        self.live_check = QCheckBox("Live Mode")
        self.live_check.setChecked(False)
        self.live_check.stateChanged.connect(self._on_live_toggled)
        grip_top.addWidget(self.live_check)
        grip_top.addStretch()
        grip_layout.addLayout(grip_top)

        grip_row = QHBoxLayout()
        self.grip_slider = ClickableSlider(Qt.Horizontal)
        self.grip_slider.setRange(0, 9200)
        self.grip_slider.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Fixed)
        self.grip_spin = QDoubleSpinBox()
        self.grip_spin.setDecimals(1)
        self.grip_spin.setRange(0.0, 92.0)
        self.grip_spin.setSingleStep(0.1)
        self.grip_spin.setFixedWidth(140)

        def grip_slider_to_val(v):
            return v / 100.0

        def val_to_grip_slider(v):
            return int(round(v * 100.0))

        self.grip_slider.valueChanged.connect(lambda v: self._on_grip_slider_changed(v, grip_slider_to_val))
        self.grip_spin.valueChanged.connect(lambda v: self._on_grip_spin_changed(v, val_to_grip_slider))
        grip_row.addWidget(self.grip_slider)
        grip_row.addWidget(self.grip_spin)
        grip_layout.addLayout(grip_row)

        ff_row = QHBoxLayout()
        ff_row.addWidget(QLabel("FF Torque (Nm):"))
        self.ff_entry = QDoubleSpinBox()
        self.ff_entry.setRange(-3.0, 3.0)
        self.ff_entry.setSingleStep(0.1)
        self.ff_entry.setValue(0.0)
        self.ff_entry.setFixedWidth(140)
        ff_row.addWidget(self.ff_entry)
        ff_row.addStretch()
        grip_layout.addLayout(ff_row)

        preset_row = QHBoxLayout()
        send_btn = QPushButton("Send Gripper")
        send_btn.setIcon(qta.icon('mdi.send'))
        send_btn.clicked.connect(self._on_send_gripper)
        close_btn = QPushButton("Close (0 mm)")
        half_btn = QPushButton("Half (46 mm)")
        open_btn = QPushButton("Open (92 mm)")
        close_btn.setProperty("secondary", True)
        half_btn.setProperty("secondary", True)
        open_btn.setProperty("secondary", True)
        send_btn.setFixedWidth(140)
        close_btn.setFixedWidth(140)
        half_btn.setFixedWidth(140)
        open_btn.setFixedWidth(140)
        close_btn.clicked.connect(lambda: self._set_gripper_preset(0.0))
        half_btn.clicked.connect(lambda: self._set_gripper_preset(46.0))
        open_btn.clicked.connect(lambda: self._set_gripper_preset(92.0))
        preset_row.addWidget(send_btn)
        preset_row.addWidget(close_btn)
        preset_row.addWidget(half_btn)
        preset_row.addWidget(open_btn)
        preset_row.addStretch()
        grip_layout.addLayout(preset_row)
        content_layout.addWidget(grip_box)
        content_layout.addStretch()

        bottom_row = QHBoxLayout()
        send_arm_btn = QPushButton("Send Arm Command")
        send_arm_btn.setIcon(qta.icon('mdi.send'))
        send_arm_btn.setFixedWidth(220)
        reset_btn = QPushButton("Reset Arm")
        reset_btn.setIcon(qta.icon('mdi.refresh'))
        reset_btn.setFixedWidth(160)
        reset_btn.setProperty("secondary", True)
        sync_btn = QPushButton("Sync Position")
        sync_btn.setIcon(qta.icon('mdi.sync'))
        sync_btn.setFixedWidth(160)
        sync_btn.setProperty("secondary", True)
        send_arm_btn.clicked.connect(self._on_send_arm)
        reset_btn.clicked.connect(self._on_reset)
        sync_btn.clicked.connect(self._on_sync)
        bottom_row.addWidget(send_arm_btn)
        bottom_row.addWidget(reset_btn)
        bottom_row.addWidget(sync_btn)
        bottom_row.addStretch()
        main_layout.addLayout(bottom_row)

        self.setStatusBar(QStatusBar(self))
        self.statusBar().showMessage("Ready", 3000)

        self.ui_timer = QTimer(self)
        self.ui_timer.setInterval(50)
        self.ui_timer.timeout.connect(self._refresh_ui_from_node)
        self.ui_timer.start()

        self.preview_timer = QTimer(self)
        self.preview_timer.setInterval(int(1000 / 30))
        self.preview_timer.timeout.connect(lambda: self.node.publish_preview_state())
        self.preview_timer.start()

        self.node.publish_preview_state(force=True)

    def _setup_menu_bar(self):
        menubar = self.menuBar()
        settings_menu = menubar.addMenu('Settings')
        theme_menu = settings_menu.addMenu('Theme')
        light_action = theme_menu.addAction('Light Theme')
        light_action.triggered.connect(lambda: self._change_theme('light'))
        dark_action = theme_menu.addAction('Dark Theme')
        dark_action.triggered.connect(lambda: self._change_theme('dark'))

    def _setup_shortcuts(self):
        reset_shortcut = QShortcut(QKeySequence("Ctrl+R"), self)
        reset_shortcut.activated.connect(self._on_reset)
        sync_shortcut = QShortcut(QKeySequence("Ctrl+S"), self)
        sync_shortcut.activated.connect(self._on_sync)
        send_arm_shortcut = QShortcut(QKeySequence("Ctrl+Return"), self)
        send_arm_shortcut.activated.connect(self._on_send_arm)
        send_gripper_shortcut = QShortcut(QKeySequence("Ctrl+G"), self)
        send_gripper_shortcut.activated.connect(self._on_send_gripper)

    def _change_theme(self, theme):
        self.current_theme = theme
        app = QApplication.instance()
        if theme == 'light':
            ThemeManager.apply_light_theme(app)
        else:
            ThemeManager.apply_dark_theme(app)
        self.statusBar().showMessage(f"Switched to {'Light' if theme == 'light' else 'Dark'} theme", 2000)

    def _read_ff(self):
        try:
            return float(self.ff_entry.value())
        except Exception:
            return 0.0

    def _on_live_toggled(self, state):
        self.live_gripper_enabled = bool(state)
        if self.live_gripper_enabled:
            self._last_gripper_pub_time = 0.0
            self.statusBar().showMessage("Gripper live mode: ON", 2000)
            mm = self.node.target_gripper_position
            self.node.send_gripper_command(mm, self._read_ff())
        else:
            self.statusBar().showMessage("Gripper live mode: OFF", 2000)

    def _on_send_gripper(self):
        send_btn = self.sender()
        send_btn.setEnabled(False)
        mm = float(self.grip_spin.value())
        ff = self._read_ff()
        self.node.send_gripper_command(mm, ff)
        self.statusBar().showMessage(f"Sent gripper: {mm:.1f}mm / FF {ff:.2f}Nm", 2500)
        QTimer.singleShot(500, lambda: send_btn.setEnabled(True))

    def _set_gripper_preset(self, mm):
        self.grip_spin.setValue(mm)
        if self.live_check.isChecked():
            ff = self._read_ff()
            self.node.send_gripper_command(mm, ff)
            self.statusBar().showMessage(f"Live preset: sent {mm:.1f}mm", 1500)

    def _on_send_arm(self):
        send_arm_btn = self.sender()
        send_arm_btn.setEnabled(False)
        positions = [float(sp.value()) for sp in self.joint_spinboxes]
        self.node.target_positions = positions
        self.node.send_joint_command(positions)
        self.statusBar().showMessage("Arm joint command sent", 2000)
        QTimer.singleShot(500, lambda: send_arm_btn.setEnabled(True))

    def _on_reset(self):
        for i, (sp, sl, tgt) in enumerate(zip(self.joint_spinboxes, self.joint_sliders, self.target_labels)):
            sp.setValue(0.0)
            sl.setValue(self._val_to_slider(i, 0.0))
            tgt.setText("Target: 0.000")
        self.grip_spin.setValue(0.0)
        self.grip_slider.setValue(0)
        self.gripper_target_label.setText("Target: 0.0")
        self.node.target_positions = [0.0] * 7
        self.node.target_gripper_position = 0.0
        self.node.publish_preview_state(force=True)
        self.statusBar().showMessage("Arm joints reset to zero", 2500)

    def _on_sync(self):
        cur = self.node.current_positions
        for i in range(min(len(self.joint_spinboxes), len(cur))):
            val = float(cur[i])
            self.joint_spinboxes[i].setValue(val)
            self.joint_sliders[i].setValue(self._val_to_slider(i, val))
            self.target_labels[i].setText(f"Target: {val:.3f}")
            self.current_labels[i].setText(f"Current: {val:.3f}")
            self.node.target_positions[i] = val
        if len(cur) >= 8:
            joint8_m = cur[7]
            gr_mm = (joint8_m / 0.046) * 92.0
            gr_mm = max(0.0, min(92.0, gr_mm))
            self.grip_spin.setValue(gr_mm)
            self.grip_slider.setValue(int(round(gr_mm * 100.0)))
            self.gripper_current_label.setText(f"Current: {gr_mm:.1f}")
            self.node.target_gripper_position = gr_mm
        self.node.publish_preview_state(force=True)
        self.statusBar().showMessage("Synced with current position (arm + gripper)", 2500)

    def _val_to_slider(self, idx, v):
        lo, hi = self.node.joint_limits[idx]
        return int(round((v - lo) / (hi - lo) * 10000.0))

    def _on_slider_changed(self, idx, value, slider_to_val, spin):
        val = slider_to_val(value)
        spin.blockSignals(True)
        spin.setValue(val)
        spin.blockSignals(False)
        self.node.target_positions[idx] = float(val)
        self.target_labels[idx].setText(f"Target: {val:.3f}")
        self.node.publish_preview_state()

    def _on_spin_changed(self, idx, value, val_to_slider, slider):
        slider.blockSignals(True)
        slider.setValue(val_to_slider(value))
        slider.blockSignals(False)
        self.node.target_positions[idx] = float(value)
        self.target_labels[idx].setText(f"Target: {value:.3f}")
        self.node.publish_preview_state()

    def _on_reset_joint(self, idx, spinbox, slider):
        spinbox.setValue(0.0)
        slider.setValue(self._val_to_slider(idx, 0.0))
        self.target_labels[idx].setText("Target: 0.000")
        self.node.target_positions[idx] = 0.0
        self.node.publish_preview_state(force=True)
        self.statusBar().showMessage(f"{self.node.joint_names[idx]} reset to zero", 1500)

    def _on_grip_slider_changed(self, value, grip_slider_to_val):
        val = grip_slider_to_val(value)
        self.grip_spin.blockSignals(True)
        self.grip_spin.setValue(val)
        self.grip_spin.blockSignals(False)
        self.node.target_gripper_position = float(val)
        self.gripper_target_label.setText(f"Target: {val:.1f}")
        self.node.publish_preview_state()
        if self.live_check.isChecked():
            now = time.monotonic()
            if (now - self._last_gripper_pub_time) >= (1.0 / self._live_gripper_rate_hz):
                ff = self._read_ff()
                self.node.send_gripper_command(val, ff)
                self._last_gripper_pub_time = now
                self.statusBar().showMessage(f"Live: sent {val:.1f}mm / FF {ff:.2f}Nm", 1200)

    def _on_grip_spin_changed(self, value, val_to_grip_slider):
        self.grip_slider.blockSignals(True)
        self.grip_slider.setValue(val_to_grip_slider(value))
        self.grip_slider.blockSignals(False)
        self.node.target_gripper_position = float(value)
        self.gripper_target_label.setText(f"Target: {value:.1f}")
        self.node.publish_preview_state()
        if self.live_check.isChecked():
            ff = self._read_ff()
            self.node.send_gripper_command(value, ff)
            self.statusBar().showMessage(f"Live: sent {value:.1f}mm / FF {ff:.2f}Nm", 1200)

    def _refresh_ui_from_node(self):
        cur = self.node.current_positions
        for i in range(min(len(self.current_labels), len(cur))):
            self.current_labels[i].setText(f"Current: {cur[i]:.3f}")
        if len(cur) >= 8:
            joint8_m = cur[7]
            gr_mm = (joint8_m / 0.046) * 92.0
            gr_mm = max(0.0, min(92.0, gr_mm))
            self.gripper_current_label.setText(f"Current: {gr_mm:.1f}")
        
        if rclpy.ok():
            self.connection_status.setPixmap(qta.icon('mdi.check-circle', color='green').pixmap(16, 16))
        else:
            self.connection_status.setPixmap(qta.icon('mdi.circle', color='red').pixmap(16, 16))

    def closeEvent(self, event):
        print("Window is closing, stopping timers and signaling threads...")
        self.ui_timer.stop()
        self.preview_timer.stop()
        self.shutdown_event.set()
        event.accept()


def start_ros_spin_thread(node: JointControlNode, shutdown_event: threading.Event):
    def ros_spin_loop():
        print("ROS spin thread started.")
        while rclpy.ok() and not shutdown_event.is_set():
            try:
                rclpy.spin_once(node, timeout_sec=0.1)
            except Exception as e:
                if rclpy.ok():
                    node.get_logger().error(f"ROS spin thread exception: {e}")
        print("ROS spin thread finished.")

    t = threading.Thread(target=ros_spin_loop, daemon=True)
    t.start()
    return t


def main():
    rclpy.init()
    node = JointControlNode()
    shutdown_event = threading.Event()
    spin_thread = start_ros_spin_thread(node, shutdown_event)

    app = QApplication(sys.argv)
    app.setFont(QFont("Inter", 14))
    ThemeManager.apply_light_theme(app)
    win = JointControlWindow(node, shutdown_event)
    win.show()

    try:
        rc = app.exec()
    except KeyboardInterrupt:
        print("Ctrl+C detected, initiating shutdown...")
    finally:
        print("Main loop finished, cleaning up...")
        shutdown_event.set()
        print("Waiting for ROS spin thread to join...")
        spin_thread.join(timeout=2.0)
        if spin_thread.is_alive():
            print("Warning: ROS spin thread did not exit gracefully.")
        print("Shutting down ROS2 node...")
        rclpy.shutdown()
        print("Shutdown complete.")
        sys.exit(rc)


if __name__ == "__main__":
    main()
